package org.cove.ape {
	import sandy.core.data.Matrix4;	

	/**
	 * @author cbra
	 */
	public class Quaternion {
		public var x:Number;
		public var y:Number;
		public var z:Number;
		public var w:Number;

		/**
		 * Constructor instantiates a new <code>Quaternion</code> object from the
		 * given list of parameters.
		 *
		 * @param x
		 *            the x value of the quaternion.
		 * @param y
		 *            the y value of the quaternion.
		 * @param z
		 *            the z value of the quaternion.
		 * @param w
		 *            the w value of the quaternion.
		 */
		public function Quaternion(x:Number = 0, y:Number = 0, z:Number = 0, w:Number = 1) {
			this.x = x;
			this.y = y;
			this.z = z;
			this.w = w;
		}
		
		public function copy(aQuaternion:Quaternion):void{
			x = aQuaternion.x;
			y = aQuaternion.y;
			z = aQuaternion.z;
			w = aQuaternion.w;
		}
		
		public function clone():Quaternion{
			return new Quaternion(x,y,z,w);
		}

		/**
		 * Sets this Quaternion to {0, 0, 0, 1}.  Same as calling set(0,0,0,1).
		 */
		public function loadIdentity():void {
			x = y = z = 0;
			w = 1;
		}

		/**
		 * <code>mult</code> multiplies this quaternion by a parameter quaternion.
		 * The result is returned as a new quaternion. It should be noted that
		 * quaternion multiplication is not cummulative so q * p != p * q.
		 *
		 * It IS safe for q and res to be the same object.
		 *
		 * @param q
		 *            the quaternion to multiply this quaternion by.
		 * @param res
		 *            the quaternion to store the result in.
		 * @return the new quaternion.
		 */
		public function mult(q:Quaternion , res:Quaternion ):Quaternion {
			if (res == null) {
				res = new Quaternion();
			}
			var qw:Number = q.w;
			var qx:Number = q.x;
			var qy:Number = q.y;
			var qz:Number = q.z;   

			res.x = x * qw + y * qz - z * qy + w * qx;
			res.y = -x * qz + y * qw + z * qx + w * qy;
			res.z = x * qy - y * qx + z * qw + w * qz;
			res.w = -x * qx - y * qy - z * qz + w * qw;
			return res;
		}

		/**
		 * <code>connect</code> multiplies this quaternion by a parameter quaternion.
		 * The result is returned as a new quaternion. It should be noted that
		 * quaternion multiplication is not cummulative so q * p != p * q.
		 *
		 * @param q
		 *            the quaternion to multiply this quaternion by.
		 * @return the new quaternion.
		 */
		public function connect( q:Quaternion):Quaternion {
			return mult(q, null);
		}

		/**
		 * <code>fromAngleAxis</code> sets this quaternion to the values specified
		 * by an angle and an axis of rotation. This method creates an object, so
		 * use fromAngleNormalAxis if your axis is already normalized.
		 *
		 * @param angle
		 *            the angle to rotate (in radians).
		 * @param axis
		 *            the axis of rotation.
		 * @return this quaternion
		 */
		public function fromAngleAxis( angle:Number, axis:Vector3f ):Quaternion {
			var normAxis:Vector3f = axis.normalize();
			fromAngleNormalAxis(angle, normAxis);
			return this;
		}

		/**
		 * <code>fromAngleNormalAxis</code> sets this quaternion to the values
		 * specified by an angle and a normalized axis of rotation.
		 *
		 * @param angle
		 *            the angle to rotate (in radians).
		 * @param axis
		 *            the axis of rotation (already normalized).
		 */
		public function fromAngleNormalAxis( angle:Number,axis:Vector3f ):Quaternion {
			if (axis.x == 0 && axis.y == 0 && axis.z == 0) {
				loadIdentity();
			} else {
				var halfAngle:Number = 0.5 * angle;
				var sin:Number = Math.sin(halfAngle);
				w = Math.cos(halfAngle);
				x = sin * axis.x;
				y = sin * axis.y;
				z = sin * axis.z;
			}
			return this;
		}

		
		/**
		 * <code>toAngleAxis</code> sets a given angle and axis to that
		 * represented by the current quaternion. The values are stored as
		 * following: The axis is provided as a parameter and built by the method,
		 * the angle is returned as a float.
		 *
		 * @param axisStore
		 *            the object we'll store the computed axis in.
		 * @return the angle of rotation in radians.
		 */
		public function toAngleAxis( axisStore:Vector3f):Number {
			var sqrLength:Number = x * x + y * y + z * z;
			var angle:Number;
			if (sqrLength == 0.0) {
				angle = 0.0;
				if (axisStore != null) {
					axisStore.x = 1.0;
					axisStore.y = 0.0;
					axisStore.z = 0.0;
				}
			} else {
				angle = (2.0 * Math.acos(w));
				if (axisStore != null) {
					var invLength:Number = (1.0 / Math.sqrt(sqrLength));
					axisStore.x = x * invLength;
					axisStore.y = y * invLength;
					axisStore.z = z * invLength;
				}
			}

			return angle;
		}

		
		/**
         * <code>toAngles</code> returns this quaternion converted to Euler
         * rotation angles (yaw,roll,pitch).<br/>
         * See http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
         * 
         * @param angles
         *            the float[] in which the angles should be stored, or null if
         *            you want a new float[] to be created
         * @return the float[] in which the angles are stored.
         */
        public function toAngles():Vector3f {
			var result:Vector3f = new Vector3f();
	
			var sqw:Number = w * w;
			var sqx:Number = x * x;
			var sqy:Number = y * y;
			var sqz:Number = z * z;
			var unit:Number = sqx + sqy + sqz + sqw; 
			// if normalized is one, otherwise
			// is correction factor
             var test:Number = x * y + z * w;
                if (test > 0.499 * unit) { // singularity at north pole
                        result.y = 2 * Math.atan2(x, w);
                        result.z = MathUtil.HALF_PI;
                        result.x = 0;
//                        angles[1] = 2 * FastMath.atan2(x, w);
//                        angles[2] = FastMath.HALF_PI;
//                        angles[0] = 0;
                } else if (test < -0.499 * unit) { // singularity at south pole
                        result.y  = -2 * Math.atan2(x, w);
                        result.z = -MathUtil.HALF_PI;
                        result.x = 0;
                } else {
                        result.y = Math.atan2(2 * y * w - 2 * x * z, sqx - sqy - sqz + sqw); // roll or heading 
                        result.z = Math.asin(2 * test / unit); // pitch or attitude
                        result.x = Math.atan2(2 * x * w - 2 * y * z, -sqx + sqy - sqz + sqw); // yaw or bank
                }
                
            return result;
        }

		/**
		 * <code>norm</code> returns the norm of this quaternion. This is the dot
		 * product of this quaternion with itself.
		 *
		 * @return the norm of the quaternion.
		 */
		public function norm():Number {
			return w * w + x * x + y * y + z * z;
		}

		
		/**
		 * <code>normalize</code> normalizes the current <code>Quaternion</code>
		 */
		public function normalize():void {
			var n:Number = 1 / Math.sqrt(norm());
			x *= n;
			y *= n;
			z *= n;
			w *= n;
		}
	
		public function toRotationMatrix(result:Matrix4):Matrix4 {

			var norm:Number = norm();
			// we explicitly test norm against one here, saving a division
			// at the cost of a test and branch.  Is it worth it?
			var s:Number = (norm == 1) ? 2 : (norm > 0) ? 2 / norm : 0;
        
			// compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
			// will be used 2-4 times each.
			var xs:Number = x * s;
			var ys:Number = y * s;
			var zs:Number = z * s;
			var xx:Number = x * xs;
			var xy:Number = x * ys;
			var xz:Number = x * zs;
			var xw:Number = w * xs;
			var yy:Number = y * ys;
			var yz:Number = y * zs;
			var yw:Number = w * ys;
			var zz:Number = z * zs;
			var zw:Number = w * zs;

			// using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here
			result.n11 = 1 - ( yy + zz );
			result.n12 = ( xy - zw );
			result.n13 = ( xz + yw );
			result.n21 = ( xy + zw );
			result.n22 = 1 - ( xx + zz );
			result.n23 = ( yz - xw );
			result.n31 = ( xz - yw );
			result.n32 = ( yz + xw );
			result.n33 = 1 - ( xx + yy );

			return result;
		}

		public function fromRotationMatrix(
			m00:Number, m01:Number, m02:Number ,
            m10:Number, m11:Number, m12:Number ,
            m20:Number, m21:Number, m22:Number):Quaternion {
			// Use the Graphics Gems code, from 
			// ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z
			// *NOT* the "Matrix and Quaternions FAQ", which has errors!
        
			// the trace is the sum of the diagonal elements; see
			// http://mathworld.wolfram.com/MatrixTrace.html
			var t:Number = m00 + m11 + m22;
			var s:Number;
			// we protect the division by s by ensuring that s>=1
			if (t >= 0) { 
				// |w| >= .5
				s = Math.sqrt(t + 1); 
				// |s|>=1 ...
				w = 0.5 * s;
				s = 0.5 / s;                 
				// so this division isn't bad
				x = (m21 - m12) * s;
				y = (m02 - m20) * s;
				z = (m10 - m01) * s;
			} else if ((m00 > m11) && (m00 > m22)) {
				s = Math.sqrt(1.0 + m00 - m11 - m22); 
				// |s|>=1
				x = s * 0.5; 
				// |x| >= .5
				s = 0.5 / s;
				y = (m10 + m01) * s;
				z = (m02 + m20) * s;
				w = (m21 - m12) * s;
			} else if (m11 > m22) {
				s = Math.sqrt(1.0 + m11 - m00 - m22); 
				// |s|>=1
				y = s * 0.5; 
				// |y| >= .5
				s = 0.5 / s;
				x = (m10 + m01) * s;
				z = (m21 + m12) * s;
				w = (m02 - m20) * s;
			} else {
				s = Math.sqrt(1.0 + m22 - m00 - m11); 
				// |s|>=1
				z = s * 0.5; 
				// |z| >= .5
				s = 0.5 / s;
				x = (m02 + m20) * s;
				y = (m21 + m12) * s;
				w = (m10 - m01) * s;
			}
        
			return this;
		}

		
		public function toSandyRotationMatrix(result:Matrix4):Matrix4 {

			var norm:Number = norm();
			// we explicitly test norm against one here, saving a division
			// at the cost of a test and branch.  Is it worth it?
			var s:Number = (norm == 1) ? 2 : (norm > 0) ? 2 / norm : 0;
        
        	// up down axis ok
//        	var x:Number = this.y;
//			var y:Number = -this.z;
//        	var z:Number = -this.x;
        	
//        	var rot90:Quaternion = new Quaternion();
//        	rot90.fromAngleAxis(MathUtil.HALF_PI, new Vector3f(0,0,1));
//        	var temp:Quaternion = this.connect(rot90);
        	
//        	rotation = currentAxisRotation.connect(rotation);
        	var x:Number = this.x;
			var y:Number = this.y;
        	var z:Number = this.z;	
        	var w:Number = this.w;
        	
			// compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
			// will be used 2-4 times each.
			var xs:Number = x * s;
			var ys:Number = y * s;
			var zs:Number = z * s;
			var xx:Number = x * xs;
			var xy:Number = x * ys;
			var xz:Number = x * zs;
			var xw:Number = w * xs;
			var yy:Number = y * ys;
			var yz:Number = y * zs;
			var yw:Number = w * ys;
			var zz:Number = z * zs;
			var zw:Number = w * zs;


			result.n21 = -(1 - ( yy + zz ));
			result.n22 = -( xy - zw );
			result.n23 = -( xz + yw );
			
			result.n11 = ( xy + zw );
			result.n12 = 1 - ( xx + zz );
			result.n13 = ( yz - xw );
			
			result.n31 = ( xz - yw );
			result.n32 = ( yz + xw );
			result.n33 = 1 - ( xx + yy );


			/*
			// using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here
			result.n11 = 1 - ( yy + zz );
			result.n12 = ( xy - zw );
			result.n13 = ( xz + yw );
			result.n21 = ( xy + zw );
			result.n22 = 1 - ( xx + zz );
			result.n23 = ( yz - xw );
			result.n31 = ( xz - yw );
			result.n32 = ( yz + xw );
			result.n33 = 1 - ( xx + yy );
			*/

			return result;
		}

		public function toString():String{
			return "x: "+x+" y: "+ y+ " z: "+z+ " w: "+w;
		}
		
		public function set(x:Number,y:Number,z:Number,w:Number):void{
			this.x = x;
			this.y = y;
			this.z = z;
			this.w = w;	
		}
	}
}
